#include "SHT40.h"

static uint8_t crc8(const uint8_t *data, int len);

/*!
 * @brief  SHT40 constructor
 */
SHT40::SHT40(void) {}

/*!
 * @brief  SHT40 destructor
 */
SHT40::~SHT40(void) {
    if (temp_sensor) {
        delete temp_sensor;
    }
    if (humidity_sensor) {
        delete humidity_sensor;
    }
}

/**
 * Initialises the I2C bus, and assigns the I2C address to us.
 *
 * @param theWire   The I2C bus to use, defaults to &Wire
 *
 * @return True if initialisation was successful, otherwise False.
 */
bool SHT40::begin(uint8_t SDAPin, uint8_t SCLPin,TwoWire *theWire) {
    if (i2c_dev) {
        delete i2c_dev; // remove old interface
    }
    theWire->begin(SDAPin,SCLPin,150000);
    i2c_dev = new Adafruit_I2CDevice(SHT40_DEFAULT_ADDR, theWire);

    if (!i2c_dev->begin()) {
        return false;
    }

    if (!reset()) {
        return false;
    }

    humidity_sensor = new SHT40_Humidity(this);
    temp_sensor = new SHT40_Temp(this);
    return true;
}

/**
 * Gets the ID register contents.
 *
 * @return The 32-bit ID register.
 */
uint32_t SHT40::readSerial(void) {
    uint8_t cmd = SHT40_READSERIAL;
    uint8_t reply[6];

    if (!i2c_dev->write(&cmd, 1)) {
        return false;
    }
    delay(10);
    if (!i2c_dev->read(reply, 6)) {
        return false;
    }

    if ((crc8(reply, 2) != reply[2]) || (crc8(reply + 3, 2) != reply[5])) {
        return false;
    }

    uint32_t serial = 0;
    serial = reply[0];
    serial <<= 8;
    serial |= reply[1];
    serial <<= 8;
    serial |= reply[3];
    serial <<= 8;
    serial |= reply[4];

    return serial;
}

/**
 * Performs a soft reset of the sensor to put it into a known state.
 * @returns True on success, false if could not communicate with chip
 */
bool SHT40::reset(void) {
    uint8_t cmd = SHT40_SOFTRESET;
    if (!i2c_dev->write(&cmd, 1)) {
        return false;
    }
    delay(1);
    return true;
}

/**************************************************************************/
/*!
    @brief  Sets the precision rating - more precise takes longer!
    @param  prec The desired precision setting, will be used during reads
*/
/**************************************************************************/
void SHT40::setPrecision(SHT40_precision_t prec) { _precision = prec; }

/**************************************************************************/
/*!
    @brief  Gets the precision rating - more precise takes longer!
    @returns  The current precision setting, will be used during reads
*/
/**************************************************************************/
SHT40_precision_t SHT40::getPrecision(void) { return _precision; }

/**************************************************************************/
/*!
    @brief  Sets the heating setting - more heating uses more power and takes
   longer
    @param  heat The desired heater setting, will be used during reads
*/
/**************************************************************************/
void SHT40::setHeater(SHT40_heater_t heat) { _heater = heat; }

/**************************************************************************/
/*!
    @brief  Gets the heating setting - more heating uses more power and takes
   longer
    @returns  The current heater setting, will be used during reads
*/
/**************************************************************************/
SHT40_heater_t SHT40::getHeater(void) { return _heater; }

/**************************************************************************/
/*!
    @brief  Gets the humidity sensor and temperature values as sensor events
    @param  humidity Sensor event object that will be populated with humidity
   data
    @param  temp Sensor event object that will be populated with temp data
    @returns true if the event data was read successfully
*/
/**************************************************************************/
bool SHT40::getEvent(sensors_event_t *humidity,
                              sensors_event_t *temp) {
    uint8_t readbuffer[6];
    uint8_t cmd = SHT40_NOHEAT_HIGHPRECISION;
    uint16_t duration = 10;

    if (_heater == SHT40_NO_HEATER) {
        if (_precision == SHT40_HIGH_PRECISION) {
            cmd = SHT40_NOHEAT_HIGHPRECISION;
            duration = 10;
        }
        if (_precision == SHT40_MED_PRECISION) {
            cmd = SHT40_NOHEAT_MEDPRECISION;
            duration = 5;
        }
        if (_precision == SHT40_LOW_PRECISION) {
            cmd = SHT40_NOHEAT_LOWPRECISION;
            duration = 2;
        }
    }

    if (_heater == SHT40_HIGH_HEATER_1S) {
        cmd = SHT40_HIGHHEAT_1S;
        duration = 1100;
    }
    if (_heater == SHT40_HIGH_HEATER_100MS) {
        cmd = SHT40_HIGHHEAT_100MS;
        duration = 110;
    }

    if (_heater == SHT40_MED_HEATER_1S) {
        cmd = SHT40_MEDHEAT_1S;
        duration = 1100;
    }
    if (_heater == SHT40_MED_HEATER_100MS) {
        cmd = SHT40_MEDHEAT_100MS;
        duration = 110;
    }

    if (_heater == SHT40_LOW_HEATER_1S) {
        cmd = SHT40_LOWHEAT_1S;
        duration = 1100;
    }
    if (_heater == SHT40_LOW_HEATER_100MS) {
        cmd = SHT40_LOWHEAT_100MS;
        duration = 110;
    }

    if (!i2c_dev->write(&cmd, 1)) {
        return false;
    }
    delay(duration);
    if (!i2c_dev->read(readbuffer, 6)) {
        return false;
    }

    if (readbuffer[2] != crc8(readbuffer, 2) ||
        readbuffer[5] != crc8(readbuffer + 3, 2))
        return false;

    float t_ticks = (uint16_t)readbuffer[0] * 256 + (uint16_t)readbuffer[1];
    float rh_ticks = (uint16_t)readbuffer[3] * 256 + (uint16_t)readbuffer[4];
    _temperature = -45 + 175 * t_ticks / 65535;
    _humidity = -6 + 125 * rh_ticks / 65535;

    _humidity = min(max(_humidity, (float)0.0), (float)100.0);

    // use helpers to fill in the events
    if (temp)
        fillTempEvent(temp);
    if (humidity)
        fillHumidityEvent(humidity);
    return true;
}

void SHT40::fillTempEvent(sensors_event_t *temp) {
    memset(temp, 0, sizeof(sensors_event_t));
    temp->version = sizeof(sensors_event_t);
    temp->sensor_id = _sensorid_temp;
    temp->type = SENSOR_TYPE_AMBIENT_TEMPERATURE;
    temp->temperature = _temperature;
}

void SHT40::fillHumidityEvent(sensors_event_t *humidity) {
    memset(humidity, 0, sizeof(sensors_event_t));
    humidity->version = sizeof(sensors_event_t);
    humidity->sensor_id = _sensorid_humidity;
    humidity->type = SENSOR_TYPE_RELATIVE_HUMIDITY;
    humidity->relative_humidity = _humidity;
}

/**
 * @brief Gets the A_Sensor object for the SHT40's humidity sensor
 *
 * @return A_Sensor*
 */
A_Sensor *SHT40::getHumiditySensor(void) {
    return humidity_sensor;
}

/**
 * @brief Gets the A_Sensor object for the SHT40's temperature sensor
 *
 * @return A_Sensor*
 */
A_Sensor *SHT40::getTemperatureSensor(void) {
    return temp_sensor;
}
/**
 * @brief  Gets the sensor1_t object describing the SHT40's humidity sensor
 *
 * @param sensor The sensor1_t object to be populated
 */
void SHT40_Humidity::getSensor(sensor1_t *sensor) {
    /* Clear the sensor1_t object */
    memset(sensor, 0, sizeof(sensor1_t));

    /* Insert the sensor name in the fixed length char array */
    strncpy(sensor->name, "SHT40_H", sizeof(sensor->name) - 1);
    sensor->name[sizeof(sensor->name) - 1] = 0;
    sensor->version = 1;
    sensor->sensor_id = _sensorID;
    sensor->type = SENSOR_TYPE_RELATIVE_HUMIDITY;
    sensor->min_delay = 0;
    sensor->min_value = 0;
    sensor->max_value = 100;
    sensor->resolution = 2;
}
/**
    @brief  Gets the humidity as a standard sensor event
    @param  event Sensor event object that will be populated
    @returns True
 */
bool SHT40_Humidity::getEvent(sensors_event_t *event) {
    _theSHT40->getEvent(event, NULL);

    return true;
}
/**
 * @brief  Gets the sensor1_t object describing the SHT40's tenperature sensor
 *
 * @param sensor The sensor1_t object to be populated
 */
void SHT40_Temp::getSensor(sensor1_t *sensor) {
    /* Clear the sensor1_t object */
    memset(sensor, 0, sizeof(sensor1_t));

    /* Insert the sensor name in the fixed length char array */
    strncpy(sensor->name, "SHT40_T", sizeof(sensor->name) - 1);
    sensor->name[sizeof(sensor->name) - 1] = 0;
    sensor->version = 1;
    sensor->sensor_id = _sensorID;
    sensor->type = SENSOR_TYPE_AMBIENT_TEMPERATURE;
    sensor->min_delay = 0;
    sensor->min_value = -40;
    sensor->max_value = 85;
    sensor->resolution = 0.3; // depends on calibration data?
}
/*!
    @brief  Gets the temperature as a standard sensor event
    @param  event Sensor event object that will be populated
    @returns true
*/
bool SHT40_Temp::getEvent(sensors_event_t *event) {
    _theSHT40->getEvent(NULL, event);

    return true;
}

/**
 * Internal function to perform an I2C write.
 *
 * @param cmd   The 16-bit command ID to send.
 */
bool SHT40::writeCommand(uint16_t command) {
    uint8_t cmd[2];

    cmd[0] = command >> 8;
    cmd[1] = command & 0xFF;

    return i2c_dev->write(cmd, 2);
}

/**
 * Internal function to perform an I2C read.
 *
 * @param cmd   The 16-bit command ID to send.
 */
bool SHT40::readCommand(uint16_t command, uint8_t *buffer,
                                 uint8_t num_bytes) {
    uint8_t cmd[2];

    cmd[0] = command >> 8;
    cmd[1] = command & 0xFF;

    return i2c_dev->write_then_read(cmd, 2, buffer, num_bytes);
}

/**
 * Performs a CRC8 calculation on the supplied values.
 *
 * @param data  Pointer to the data to use when calculating the CRC8.
 * @param len   The number of bytes in 'data'.
 *
 * @return The computed CRC8 value.
 */
static uint8_t crc8(const uint8_t *data, int len) {
    /*
     *
     * CRC-8 formula from page 14 of SHT spec pdf
     *
     * Test data 0xBE, 0xEF should yield 0x92
     *
     * Initialization data 0xFF
     * Polynomial 0x31 (x8 + x5 +x4 +1)
     * Final XOR 0x00
     */

    const uint8_t POLYNOMIAL(0x31);
    uint8_t crc(0xFF);

    for (int j = len; j; --j) {
        crc ^= *data++;

        for (int i = 8; i; --i) {
            crc = (crc & 0x80) ? (crc << 1) ^ POLYNOMIAL : (crc << 1);
        }
    }
    return crc;
}